/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Vector3.h"

namespace RobotPlant { namespace Math{
	Vector3::Vector3(void)
	{
		_datas[0]=_datas[1]=_datas[2] = 0.0;
	}

	Vector3::~Vector3(void)
	{
	}

	Vector3::Vector3(const Vector3 & arg)
	{
		_datas[0] = arg._datas[0];
		_datas[1] = arg._datas[1];
		_datas[2] = arg._datas[2];
	}

	Vector3::Vector3(const double x,const double y, const double z)
	{
		_datas[0] = x;
		_datas[1] = y;
		_datas[2] = z;
	}

	double& Vector3::operator [] (const int index)
	{
		if((index < 0) || (index > 2))
			throw(new std::exception("Argument index must be between 0 and 2."));

		return _datas[index];
	}

	double Vector3::operator [] ( const int index ) const
	{
		if((index < 0) || (index > 2))
			throw(new std::exception("Argument index must be between 0 and 2."));

		return _datas[index];
	}

	Vector3& Vector3::operator = (const Vector3 & arg)
	{
		_datas[0] = arg._datas[0];
		_datas[1] = arg._datas[1];
		_datas[2] = arg._datas[2];
		return *this;
	}

	Vector3& Vector3::operator += (const Vector3 & arg)
	{
		_datas[0] += arg._datas[0];
		_datas[1] += arg._datas[1];
		_datas[2] += arg._datas[2];
		return *this;
	}

	Vector3& Vector3::operator -= (const Vector3 & arg)
	{
		_datas[0] -= arg._datas[0];
		_datas[1] -= arg._datas[1];
		_datas[2] -= arg._datas[2];
		return *this;
	}


	bool operator == ( const Vector3& lhs, const Vector3& rhs )
	{
		return Equal(lhs,rhs);
	}

	bool operator != ( const Vector3& lhs, const Vector3& rhs ) 
	{
		return !Equal(lhs,rhs);
	}

	Vector3 operator + (const Vector3& lhs,const Vector3& rhs)
	{
		Vector3 tmp;
		tmp._datas[0] = lhs._datas[0] + rhs._datas[0];
		tmp._datas[1] = lhs._datas[1] + rhs._datas[1];
		tmp._datas[2] = lhs._datas[2] + rhs._datas[2];
		return tmp;
	}

	Vector3 operator - (const Vector3& lhs,const Vector3& rhs)
	{
		Vector3 tmp;
		tmp._datas[0] = lhs._datas[0] - rhs._datas[0];
		tmp._datas[1] = lhs._datas[1] - rhs._datas[1];
		tmp._datas[2] = lhs._datas[2] - rhs._datas[2];
		return tmp;
	}

	Vector3 operator - (const Vector3& arg)
	{
		return Vector3(- arg._datas[0], - arg._datas[1], - arg._datas[2]);
	}

	Vector3 CrossProd( const Vector3& lhs, const Vector3& rhs)
	{
		Vector3 tmp;
		tmp._datas[0] = lhs._datas[1] * rhs._datas[2] - lhs._datas[2] * rhs._datas[1];
		tmp._datas[1] = lhs._datas[2] * rhs._datas[0] - lhs._datas[0] * rhs._datas[2];
		tmp._datas[2] = lhs._datas[0] * rhs._datas[1] - lhs._datas[1] * rhs._datas[0];
		return tmp;
	}

	double operator * (const Vector3& lhs,const Vector3& rhs)
	{
		return rhs[0]*lhs[0]+rhs[1]*lhs[1]+rhs[2]*lhs[2];
	}

	Vector3 operator * ( const double lhs, const Vector3& rhs ) 
	{
		Vector3 tmp;
		tmp._datas[0] = lhs * rhs._datas[0];
		tmp._datas[1] = lhs * rhs._datas[1];
		tmp._datas[2] = lhs * rhs._datas[2];
		return tmp;
	}

	Vector3 operator * ( const Vector3& lhs, const double rhs ) 
	{
		Vector3 tmp;
		tmp._datas[0] = lhs._datas[0] * rhs;
		tmp._datas[1] = lhs._datas[1] * rhs;
		tmp._datas[2] = lhs._datas[2] * rhs;
		return tmp;
	}

	Vector3 operator / ( const Vector3& lhs, const double rhs ) 
	{
		Vector3 tmp;
		tmp._datas[0] = lhs._datas[0] / rhs;
		tmp._datas[1] = lhs._datas[1] / rhs;
		tmp._datas[2] = lhs._datas[2] / rhs;
		return tmp;
	}

	void Vector3::ReverseSign()
	{
		_datas[0] = -_datas[0];
		_datas[1] = -_datas[1];
		_datas[2] = -_datas[2];
	}

	void Vector3::SetZero() 
	{
		_datas[0] = 0;
		_datas[1] = 0;
		_datas[2] = 0;
	}

	Vector3 Vector3::Zero()
	{
		return Vector3(0,0,0);
	}

	void Vector3::Normalize()
	{
		double v = this->Norm();
		if (v <= 0) 
		{
			throw(new std::exception("This vector's norm is <= 0."));
		}

		*this = (*this)/v;
	}

	double Vector3::Norm() const
	{
		// do some effort not to lose precision
		double tmp1;
		double tmp2;
		tmp1 = fabs(_datas[0]);
		tmp2 = fabs(_datas[1]);
		if (tmp1 >= tmp2) {
			tmp2=fabs(_datas[2]);
			if (tmp1 >= tmp2) {
				if (tmp1 == 0) {
					// only to everything exactly zero case, all other are handled correctly
					return 0;
				}
				return tmp1 * sqrt(1 + sqr(_datas[1]/_datas[0]) + sqr(_datas[2]/_datas[0]));
			} else {
				return tmp2 * sqrt(1 + sqr(_datas[0]/_datas[2]) + sqr(_datas[1]/_datas[2]));
			}
		} else {
			tmp1=fabs(_datas[2]);
			if (tmp2 > tmp1) {
				return tmp2 * sqrt(1 + sqr(_datas[0]/_datas[1]) + sqr(_datas[2]/_datas[1]));
			} else {
				return tmp1 * sqrt(1 + sqr(_datas[0]/_datas[2]) + sqr(_datas[1]/_datas[2]));
			}
		}
	}

	//void Vector3::Set2DXY(const Vector2& v)
	//{
	//	// a 3D Vector3 where the 2D Vector3 v is put in the XY plane
	//	_datas[0]=v[0];
	//	_datas[1]=v[1];
	//	_datas[2]=0;

	//}

	//void Vector3::Set2DYZ(const Vector2& v)
	//{
	//	// a 3D Vector3 where the 2D Vector3 v is put in the YZ plane
	//	_datas[0]=0;
	//	_datas[1]=v[0];
	//	_datas[2]=v[1];
	//}

	//void Vector3::Set2DZX(const Vector2& v)
	//{
	//	// a 3D Vector3 where the 2D Vector3 v is put in the ZX plane
	//	_datas[0]=v[1];
	//	_datas[1]=0;
	//	_datas[2]=v[0];
	//}

	//void Vector3::Set2DPlane(const Frame3& frame,const Vector2& v_XY)
	//{
	//	// a 3D Vector3 where the 2D Vector3 v is put in the XY plane of the frame
	//	// frame.
	//	Vector3 tmp_XY;
	//	tmp_XY.Set2DXY(v_XY);
	//	tmp_XY = frame * tmp_XY;
	//}

	bool Equal(const Vector3& a,const Vector3& b,const double eps) 
	{
		return (Equal(a._datas[0],b._datas[0],eps) &&
			Equal(a._datas[1],b._datas[1],eps) &&
			Equal(a._datas[2],b._datas[2],eps));
	}

	Vector3 AddDelta(const Vector3& a,const Vector3& da,const double dt)
	{
		return a + da * dt;
	}

	Vector3 Diff(const Vector3& a,const Vector3& b,const double dt)
	{
		return ( b-a ) / dt;
	}

	void Random(Vector3& a) 
	{
		Random(a[0]);
		Random(a[1]);
		Random(a[2]);
	}
	void PosRandom(Vector3& a) 
	{
		PosRandom(a[0]);
		PosRandom(a[1]);
		PosRandom(a[2]);
	}
}}
